#include <glog/logging.h>
#include <yaml-cpp/yaml.h>
#include <csignal>
#include <string>
#include "basic/LoopFunc.h"
#include "ros/Manager.h"

std::shared_ptr<Manager> manager;
int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    FLAGS_log_dir = "/home/yhs/log";
    google::ParseCommandLineFlags(&argc, &argv, true);
    
    std::string config_yaml = "./config/config.yaml";
    if (argc == 2) {
        config_yaml = argv[1];
    }

    LOG(INFO) << "[MIDWARE] Load yaml from " << config_yaml;
    auto yaml = YAML::LoadFile(config_yaml);
    uint16_t udp_server_port = 0;
    uint16_t udp_client_port = 0;
    std::string curl_ip = "192.168.0.102";
    try {
        udp_server_port = yaml["UDP_SERVER_PORT"].as<uint16_t>();
        udp_client_port = yaml["UDP_CLIENT_PORT"].as<uint16_t>();
    } catch (...) {
        LOG(ERROR) << "[MIDWARE] Failed to parse yaml";
        std::exit(EXIT_FAILURE);
    }

    ros::init(argc, argv, "ros_midware");
    auto nh = std::make_shared<ros::NodeHandle>("~");
    manager = std::make_shared<Manager>(nh, udp_server_port, udp_client_port);
    manager->thread_UDPServer();

    // 根据 JSON 执行对应操作 0.2s
    LoopFunc loop_robotOp("loop_robotOp", 0.1, 2, std::bind(&Manager::thread_RobotOp, manager));
    // UDP 发送
    LoopFunc loop_udpSend("loop_udpSend", 1, 2, std::bind(&Manager::thread_UDPSend, manager));

    loop_robotOp.detach();
    loop_udpSend.detach();
    ros::AsyncSpinner spinner(0);
    spinner.start();
    ros::waitForShutdown();

    ros::shutdown();
    manager->stop();
    // Final
    return EXIT_SUCCESS;
}